/*
 * patio2.c
 *
 *  Created on: May 26, 2021
 *      Author: kirk_
 */

#include "patio2.h"
#include "openmv.h"
#include "movement.h"
#include "direction.h"
#include "distance.h"
#include "light.h"
#include "servo.h"
#include "display.h"

#define Patio2_Task1 1
#define Patio2_Task2 1
#define Patio2_Task3 1

#define Patio2_RunInterval 888U

#if Patio2_Task1

#endif

#if Patio2_Task2
#define Patio2_Task2_PreferredDistance  300U
#define Patio2_Task2_PreferredDistance2 500U
#define Patio2_Task2_LongRun           3600U
#endif

#if Patio2_Task3
#define Patio2_Task3_PreferredDistance  300U
#define Patio2_Task3_LongRun          12000U
#endif

void Patio2_Run(int8_t meters)
{
	int i;

	void(*Run)(uint32_t);
	if(meters == 0)
		return;
	/* Set direction */
	if(meters > 0)
	{
		Run = MoveForward;
	}
	else
	{
		Run = MoveBackward;
		meters = -meters;
	}
	/* Slow start */
	for(i=1;i<5;i++)
	{
		Run(i*100);
		osDelay(Patio2_RunInterval);
	}
	meters --;
	/* Fast run */
	while(meters>0)
	{
		Run(500);
		osDelay(Patio2_RunInterval);
		Run(500);
		osDelay(Patio2_RunInterval);
		meters--;
	}
	/* Finished */
	return;
}

void Patio2_RunUsmart(uint8_t direction, uint8_t meters)
{
	int8_t tmp;
	if(meters > 0x7F)
		meters = 0x7F;
	tmp = meters;
	if(!direction)
		tmp = -tmp;
	Patio2_Run(tmp);
}

void StartPatio2Task(void *argument)
{
	double angle1, angle2;
	uint8_t turn_state;
	int32_t distance;
	uint32_t d1, d2, utmp, longrun;
	uint8_t state1, state2;
	UNUSED(angle1);
	UNUSED(angle2);
	UNUSED(state1);
	UNUSED(state2);
	UNUSED(d1);
	UNUSED(d2);
	UNUSED(utmp);
	UNUSED(longrun);
	UNUSED(distance);
	UNUSED(turn_state);
	FishFoodReset();
	osThreadSuspend(Patio2TaskHandle);
	PRINTF("Entering Patio 2...\r\n");

	//RunForwardAlong();
	//while(1)osDelay(1);
	/* Check button */
	Light_Turn(0,(Button1_GPIO_Port->IDR & Button1_Pin) == 0);
	Light_Turn(2,(Button2_GPIO_Port->IDR & Button2_Pin) == 0);
	if(((Button1_GPIO_Port->IDR & Button1_Pin) == 0) && ((Button2_GPIO_Port->IDR & Button2_Pin) == 0))
		goto Patio2_Label_Task4;
	else if((Button2_GPIO_Port->IDR & Button2_Pin) == 0)
		goto Patio2_Label_Task3;
	else if((Button1_GPIO_Port->IDR & Button1_Pin) == 0)
		goto Patio2_Label_Task2;
	else
		goto Patio2_Label_Task1;

Patio2_Label_Task1:
#if Patio2_Task1
	/* Task 1 */
	PRINTF("Patio2 Task1.\r\n");
	/* Run */
	OpenMVReset();
	Patio2_Run(5);
	OpenMVCtl(2);
	osDelay(1000);
	OpenMVSet();
	osDelay(2000);
	/* Set OpenMV */
	/* Wait */
	for(;;)
	{
		/* Wait flag */
		while(!osThreadFlagsWait((uint32_t)1<<OpenMV_Task_Color, osFlagsWaitAll, 0x1FFFF))
			osDelay(1);
		osThreadFlagsClear((uint32_t)1<<OpenMV_Task_Color);
		/* Check command */
		turn_state = control_c;
		if(turn_state)
			break;
	}
	/* Reset OpenMV */
	OpenMVCtl(0);
	/* Turn to target */
	TurnCW(turn_state * 45);
	osDelay(1500);
	switch(turn_state)
	{
	case 1:
		Patio2_Run(3);
		Move(200);
		osDelay(2500);
		TurnCW(45);
		osDelay(2500);
		Patio2_Run(1);
		osDelay(1500);
		TurnCW(45);
		osDelay(1500);
		Patio2_Run(3);
		Move(200);
		osDelay(2500);
		TurnCCW(45);
		break;
	case 2:
		Patio2_Run(5);
		Move(500);
		break;
	case 3:
		Patio2_Run(3);
		Move(200);
		osDelay(2500);
		TurnCCW(45);
		osDelay(2500);
		Patio2_Run(1);
		osDelay(1500);
		TurnCCW(45);
		osDelay(1500);
		Patio2_Run(3);
		Move(200);
		osDelay(2500);
		TurnCW(45);
	}
	osDelay(1000);
	Patio2_Run(5);
	osDelay(4000);
#endif
Patio2_Label_Task2:
#if Patio2_Task2
	/* Task 2 */
	PRINTF("Patio2 Task2.\r\n");
	/* Test distance */
	distance = DistanceAverage(0,10,100);
	distance -= (int32_t)Patio2_Task2_PreferredDistance;
	Move(distance);
	/* Wait steady */
	osDelay(3000);
	/* Turn */
	TurnCW(90);
	osDelay(1800);
	Patio2_Run(-1);
	/* Adjust */
	DistanceAdjust();
	/* Wait steady */
	TurnCCW(90);
	osDelay(2000);
	/* Distance */
	distance = DistanceAverage(0,10,100);
	distance -= (int32_t)Patio2_Task2_PreferredDistance2;
	Move(distance);
	/* Wait */
	osDelay(2000);
	/* Run */
	TurnCW(90);
	osDelay(1500);
	Patio2_Run(-3);
	osDelay(1000);
	while(Distance_Get(1)<2000 && Distance_Get(2)<2000)
	{
		MoveBackward(400);
		osDelay(800);
	}
	MoveBackward(200);
	osDelay(1000);

	TurnCW(90);
	osDelay(1500);
	Patio2_Run(-2);
	osDelay(2500);
	/* New position */
	TurnCCW(90);
	osDelay(1500);
	distance = DistanceAverage(0,10,100);
	distance -= (int32_t)Patio2_Task2_PreferredDistance;
	Move(distance);
	/* Wait steady */
	osDelay(3000);
	TurnCW(90);
	osDelay(1800);

	/* Adjust */
	DistanceAdjust();

	/* Check distance */
	DistanceGetAverage(8,100,&d1,&d2);
	utmp = d1 + d2 + 1;
	utmp >>= 1;
	/* Turn */
	TurnCW(90);
	TurnCCW(6);
	osDelay(1800);
	longrun = Patio2_Task2_LongRun;
	longrun -= utmp;
	utmp = longrun % 1000;
	longrun /= 1000;
	Patio2_Run((int8_t)longrun);
	osDelay(800);
	Move(utmp);
	osDelay(800);
	RunForwardAlong();
	osDelay(4000);
	TurnCW(90);
	osDelay(4000);
	RunForwardAlong();
	osDelay(4000);
	TurnCCW(90);
	osDelay(4000);
	/* Fish food */
	FishFoodSet();
	osDelay(500);
	FishFoodReset();
	osDelay(500);
	FishFoodSet();
	osDelay(500);
	FishFoodReset();
#endif
Patio2_Label_Task3:
#if Patio2_Task3
	/* Task 3 */
	PRINTF("Patio2 Task3.\r\n");
	TurnCW(90);
	osDelay(4000);
	state1 = DistanceGetAngle(&angle1);
	TurnCW(90);
	osDelay(4000);
	state2 = DistanceGetAngle(&angle2);
	if(!state1)angle1 = 0.00;
	if(!state2)angle2 = 0.00;
	angle1 += angle2;
	if(state1 || state2)angle1 /= (double)(state1+state2);
	angle1 /= PI/180.00;
	Turn(-angle1);
	Turn(-10.00);
	Patio2_Run(16);
#if 0
	Patio2_Run(12);
	osDelay(1000);
	/* New position */
	distance = DistanceAverage(0,10,100);
	distance -= (int32_t)240;
	Move(distance);
	/* Wait steady */
	osDelay(3000);
	TurnCW(90);
	osDelay(800);
	/* Adjust */
	DistanceAdjust();
	TurnCCW(90);
	Patio2_Run(-1);
	/* To final */
	osDelay(1000);
	TurnCW(90);
	osDelay(1000);
	Patio2_Run(-3);
	osDelay(1000);
	TurnCW(90);
	osDelay(1000);
	Patio2_Run(-4);
#endif
	/* Finish */
	print_team();
#endif
Patio2_Label_Task4:
	/* Finished task */
	PRINTF("Finished patio 2.\r\n");
	/* Infinite loop */
	for(;;)
	{
		Light_Turn(0,2);
		osDelay(2000);
	}
	/* Exit */
	osThreadExit();
	return;
}

void FishFoodSet(void)
{
	Servo_Turn(1,38);
}

void FishFoodReset(void)
{
	Servo_Turn(1,78);
}

void RunForwardAlong(void)
{
	double angle;
	uint8_t state;
	double min;
	double max;

	while(Distance_Get(0)>500)
	{
		state = DistanceGetAngle(&angle);
		if(fabs(angle)>PI/8.00)
			state = 0;
		if(state)
		{
			min = cos(angle) * (Distance_Get(1) + Distance_Get(2));
			angle /= PI/180.00;
			max = min;
			min -= 450.00;
			max -= 640.00;
			if(min < 0)
				angle += ((float)min)/64.00;
			else if(max > 0)
			{
				if(max < 400)
					angle += ((float)max)/90.00;
				else
					angle += 3.33;
			}
		}
		else
		{
			angle = 0.08;
		}
		Turn(-angle);
		Move(150);
		osDelay(200);
		Move(150);
		osDelay(600);
		PRINTF("%lu\r\n", Distance_Get(0));
	}
	Move(-240);
	TurnCCW(5);
}

void RunBackwardAlong(void)
{
	double angle;
	uint8_t state;
	int min;
	int max;

	state = DistanceGetAngle(&angle);
	while(state)
	{
		if(fabs(angle)>PI/8.00)
			angle = 0.00;
		min = cos(angle) * (Distance_Get(1) + Distance_Get(2));
		angle /= PI/180.00;
		max = min;
		min -= 200;
		max -= 300;
		if(min < 0)
			angle -= ((float)min)/40.00;
		else if(max > 0)
		{
			if(max < 300)
				angle -= ((float)max)/60.00;
			else
				angle -= 5.00;
		}

		Turn(-angle);
		Move(-180);
		osDelay(400);
		Move(-180);
		osDelay(400);
		state = DistanceGetAngle(&angle);
	}
}
